using BepuPhysics.Constraints;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;

namespace BepuPhysics.CollisionDetection
{
    /// <summary>
    /// 当收到新约束的通知时,会立即将其添加到求解器。
    /// </summary>
    public partial class NarrowPhase<TCallbacks>
    {
        public struct PendingConstraintAddCache
        {
            BufferPool pool;
            struct PendingConstraint<TBodyHandles, TDescription, TContactImpulses> where TDescription : unmanaged, IConstraintDescription<TDescription>
            {
                // 请注意内存顺序。首先是可冲突对;确定性刷新依赖于内存布局来对挂起的约束进行排序。
                public CollidablePair Pair;
                public PairCacheIndex ConstraintCacheIndex;
                public TBodyHandles BodyHandles;
                public TDescription ConstraintDescription;
                public TContactImpulses Impulses;
            }

            internal Buffer<UntypedList> pendingConstraintsByType;
            internal Buffer<Buffer<ushort>> speculativeBatchIndices;
            int minimumConstraintCountPerCache;

            public PendingConstraintAddCache(BufferPool pool, int minimumConstraintCountPerCache = 128)
            {
                this.pool = pool;
                pool.TakeAtLeast(PairCache.CollisionConstraintTypeCount, out pendingConstraintsByType);
                // 使用前必须清除内存,避免垃圾数据滞留。
                pendingConstraintsByType.Clear(0, PairCache.CollisionConstraintTypeCount);
                this.minimumConstraintCountPerCache = minimumConstraintCountPerCache;
                speculativeBatchIndices = new Buffer<Buffer<ushort>>();
            }

            public unsafe void AddConstraint<TBodyHandles, TDescription, TContactImpulses>(int manifoldConstraintType,
                ref CollidablePair pair, PairCacheIndex constraintCacheIndex, TBodyHandles bodyHandles, ref TDescription constraintDescription, ref TContactImpulses impulses)
                where TDescription : unmanaged, IConstraintDescription<TDescription>
            {
                ref var cache = ref pendingConstraintsByType[manifoldConstraintType];
                var byteIndex = cache.Allocate<PendingConstraint<TBodyHandles, TDescription, TContactImpulses>>(minimumConstraintCountPerCache, pool);
                ref var pendingAdd = ref Unsafe.AsRef<PendingConstraint<TBodyHandles, TDescription, TContactImpulses>>(cache.Buffer.Memory + byteIndex);
                pendingAdd.Pair = pair;
                pendingAdd.ConstraintCacheIndex = constraintCacheIndex;
                pendingAdd.BodyHandles = bodyHandles;
                pendingAdd.ConstraintDescription = constraintDescription;
                pendingAdd.Impulses = impulses;
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public static unsafe void SequentialAddToSimulation<TBodyHandles, TDescription, TContactImpulses>(ref UntypedList list, int narrowPhaseConstraintTypeId, Simulation simulation, PairCache pairCache)
                where TDescription : unmanaged, IConstraintDescription<TDescription>
            {
                if (list.Buffer.Allocated)
                {
                    ref var start = ref Unsafe.As<byte, PendingConstraint<TBodyHandles, TDescription, TContactImpulses>>(ref *list.Buffer.Memory);
                    Debug.Assert(list.Buffer.Length >= Unsafe.SizeOf<PendingConstraint<TBodyHandles, TDescription, TContactImpulses>>() * list.Count);
                    Debug.Assert(list.ByteCount == Unsafe.SizeOf<PendingConstraint<TBodyHandles, TDescription, TContactImpulses>>() * list.Count);
                    for (int i = 0; i < list.Count; ++i)
                    {
                        ref var add = ref Unsafe.Add(ref start, i);
                        // AsPointer在这里不是GC漏洞;它来自非托管内存。
                        var handle = simulation.Solver.Add(new Span<BodyHandle>(Unsafe.AsPointer(ref add.BodyHandles), typeof(TBodyHandles) == typeof(TwoBodyHandles) ? 2 : 1), ref add.ConstraintDescription);
                        pairCache.CompleteConstraintAdd(simulation.NarrowPhase, simulation.Solver, ref add.Impulses, add.ConstraintCacheIndex, handle, ref add.Pair);
                    }
                }
            }

            /// <summary>
            /// 将挂起的约束刷新到仿真中,而无需任何形式的同步。相加按歧管生成的顺序发生。
            /// 如果接触流形生成是确定性的,则此相加的结果将是确定性的。
            /// </summary>
            internal void FlushSequentially(Simulation simulation, PairCache pairCache)
            {
                var accessors = simulation.NarrowPhase.contactConstraintAccessors;
                for (int i = 0; i < accessors.Length; ++i)
                {
                    accessors[i]?.FlushSequentially<TCallbacks>(ref pendingConstraintsByType[i], i, simulation, pairCache);
                }
            }


            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            static unsafe void AddToSimulationSpeculative<TBodyHandles, TDescription, TContactImpulses>(
                ref PendingConstraint<TBodyHandles, TDescription, TContactImpulses> constraint, int batchIndex, Simulation simulation, PairCache pairCache)
                where TDescription : unmanaged, IConstraintDescription<TDescription>
            {
                // 此函数完全负责Simulation.Add执行的操作,以及在配对缓存中完成约束添加的需要。
                // 1)分配在求解器批处理中,类型为批处理。
                // 2)将约束描述应用于类型Batch中分配的槽。
                // 3)将约束添加到两个Body列表中。
                // 4)将添加通知配对缓存。
                // 为了简单起见,所有这些都是一起完成的。您可以将约束应用程序和配对缓存通知推到不同的阶段-它们是
                // 完全并行且独立于其他工作(只要没有调整类型批大小的危险！)。
                // 我们目前不会将其放在多线程作业中,因为：
                // 1)PairCache.CompleteConstraintAdd不能与可能调整类型批大小的任务并行执行。添加约束可以做到这一点。
                // 2)说明应用程序也可能被类型批处理大小损坏。
                // 3)Body Constraint List Add利用主缓冲池,在批量调整类型等情况下,也可以通过添加约束来使用主缓冲池。
                // 4)延迟约束添加需要将句柄缓存到内存,这有点愚蠢,因为这已经完成了一半的工作。
                // 5)说明应用仅占该功能成本的5-10%左右。
                // 如果这些假设中的任何一个改变了,或者如果我们发现自己因为这些决定而在线程化伸缩中确实受到了伤害,我们可以重新审视设计。

                // 此外,此函数之所以打破Simulation.Add的各个职责,只是因为我们执行了一次PrePass,其中对批处理槽进行了最佳猜测
                // 都是经过计算的。为了利用这些最佳猜测,我们基本上创建了一个自定义重载。(事实上,如果您最终需要在其他地方使用它,您可能应该这样做
                // 取而代之的是自定义重载！)

                ConstraintHandle constraintHandle;
                ConstraintReference reference;
                var handles = new Span<BodyHandle>(Unsafe.AsPointer(ref constraint.BodyHandles), typeof(TBodyHandles) == typeof(TwoBodyHandles) ? 2 : 1);
                while (!simulation.Solver.TryAllocateInBatch(
                    default(TDescription).ConstraintTypeId, batchIndex,
                    handles, out constraintHandle, out reference))
                {
                    // 如果一个批处理索引失败,只需尝试下一个。这最终肯定会奏效。
                    ++batchIndex;
                }
                simulation.Solver.ApplyDescriptionWithoutWaking(ref reference, ref constraint.ConstraintDescription);
                ref var aLocation = ref simulation.Bodies.HandleToLocation[handles[0].Value];
                Debug.Assert(aLocation.SetIndex == 0, "By the time we flush new constraints into the solver, all associated islands should be awake.");
                simulation.Bodies.AddConstraint(aLocation.Index, constraintHandle, 0);
                if (typeof(TBodyHandles) == typeof(TwoBodyHandles))
                {
                    ref var bLocation = ref simulation.Bodies.HandleToLocation[handles[1].Value];
                    Debug.Assert(bLocation.SetIndex == 0, "By the time we flush new constraints into the solver, all associated islands should have be awake.");
                    simulation.Bodies.AddConstraint(bLocation.Index, constraintHandle, 1);
                }
                pairCache.CompleteConstraintAdd(simulation.NarrowPhase, simulation.Solver, ref constraint.Impulses, constraint.ConstraintCacheIndex, constraintHandle, ref constraint.Pair);
            }



            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public static unsafe void SequentialAddToSimulationSpeculative<TBodyHandles, TDescription, TContactImpulses>(
                ref UntypedList list, int narrowPhaseConstraintTypeId, ref Buffer<Buffer<ushort>> speculativeBatchIndices, Simulation simulation, PairCache pairCache)
                where TDescription : unmanaged, IConstraintDescription<TDescription>
            {
                if (list.Buffer.Allocated)
                {
                    ref var start = ref Unsafe.As<byte, PendingConstraint<TBodyHandles, TDescription, TContactImpulses>>(ref *list.Buffer.Memory);
                    Debug.Assert(list.Buffer.Length >= Unsafe.SizeOf<PendingConstraint<TBodyHandles, TDescription, TContactImpulses>>() * list.Count);
                    Debug.Assert(list.ByteCount == Unsafe.SizeOf<PendingConstraint<TBodyHandles, TDescription, TContactImpulses>>() * list.Count);
                    ref var speculativeBatchIndicesForType = ref speculativeBatchIndices[narrowPhaseConstraintTypeId];
                    for (int i = 0; i < list.Count; ++i)
                    {
                        AddToSimulationSpeculative(ref Unsafe.Add(ref start, i), (int)speculativeBatchIndicesForType[i], simulation, pairCache);
                    }
                }
            }

            /// <summary>
            /// 将挂起的约束刷新到仿真中,而无需任何形式的同步。相加按歧管生成的顺序发生。
            /// 如果接触流形生成是确定性的,则此相加的结果将是确定性的。
            /// </summary>
            internal void FlushWithSpeculativeBatches(Simulation simulation, ref PairCache pairCache)
            {
                var accessors = simulation.NarrowPhase.contactConstraintAccessors;
                for (int i = 0; i < accessors.Length; ++i)
                {
                    accessors[i]?.FlushWithSpeculativeBatches<TCallbacks>(ref pendingConstraintsByType[i], i, ref speculativeBatchIndices, simulation, pairCache);
                }
            }


            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public static unsafe void DeterministicAdd<TBodyHandles, TDescription, TContactImpulses>(
                int typeIndex, ref SortConstraintTarget target, OverlapWorker[] overlapWorkers, Simulation simulation, ref PairCache pairCache)
                where TDescription : unmanaged, IConstraintDescription<TDescription>
            {
                ref var cache = ref overlapWorkers[target.WorkerIndex].PendingConstraints;
                ref var constraint = ref Unsafe.As<byte, PendingConstraint<TBodyHandles, TDescription, TContactImpulses>>(
                   ref *(cache.pendingConstraintsByType[typeIndex].Buffer.Memory + target.ByteIndexInCache));
                // TODO：检查此iDiv的成本。它会出现在每个单独的约束添加中,所以它实际上可能会出现.
                // 问题是,在非字节索引的目标中包含另一点元数据是否更慢。
                // 如果您愿意进行位打包,我们可以使用10-12位作为Worker索引,使用20-22位作为索引。4096名工人和100万个新的限制可能已经足够了…
                var index = target.ByteIndexInCache / Unsafe.SizeOf<PendingConstraint<TBodyHandles, TDescription, TContactImpulses>>();
                AddToSimulationSpeculative(ref constraint, cache.speculativeBatchIndices[typeIndex][index], simulation, pairCache);
            }


            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            internal unsafe void SpeculativeConstraintBatchSearch(Solver solver, int typeIndex, int start, int end)
            {
                ref var list = ref pendingConstraintsByType[typeIndex];
                Debug.Assert(list.Buffer.Allocated, "The target region should be allocated, or else the job scheduler is broken.");
                Debug.Assert(list.Count > 0);
                int byteIndex = start * list.ElementSizeInBytes;
                int bodyCount = ExtractContactConstraintBodyCount(typeIndex);
                ref var speculativeBatchIndicesForType = ref speculativeBatchIndices[typeIndex];
                for (int i = start; i < end; ++i)
                {
                    speculativeBatchIndicesForType[i] = (ushort)solver.FindCandidateBatch(new Span<BodyHandle>(list.Buffer.Memory + byteIndex, bodyCount));
                    byteIndex += list.ElementSizeInBytes;
                }
            }


            // 请注意,处理与冲洗是分开的。这是必需的-访问缓冲池是一个潜在的争用条件。同时执行这些返回
            // 新鲜度检查器会导致错误。
            public void Dispose()
            {
                for (int i = 0; i < PairCache.CollisionConstraintTypeCount; ++i)
                {
                    if (pendingConstraintsByType[i].Buffer.Allocated)
                        pool.Return(ref pendingConstraintsByType[i].Buffer);
                }
                pool.Return(ref pendingConstraintsByType);
            }

            internal void AllocateForSpeculativeSearch()
            {
                pool.TakeAtLeast(PairCache.CollisionConstraintTypeCount, out speculativeBatchIndices);
                speculativeBatchIndices.Clear(0, PairCache.CollisionConstraintTypeCount);
                for (int i = 0; i < PairCache.CollisionConstraintTypeCount; ++i)
                {
                    ref var typeList = ref pendingConstraintsByType[i];
                    if (typeList.Buffer.Allocated)
                    {
                        Debug.Assert(typeList.Count > 0);
                        pool.TakeAtLeast(typeList.Count, out speculativeBatchIndices[i]);
                    }
                }
            }

            internal void DisposeSpeculativeSearch()
            {
                for (int i = 0; i < PairCache.CollisionConstraintTypeCount; ++i)
                {
                    ref var indices = ref speculativeBatchIndices[i];
                    if (indices.Allocated)
                    {
                        pool.Return(ref indices);
                    }
                }
                pool.Return(ref speculativeBatchIndices);
            }
            internal int CountConstraints()
            {
                int count = 0;
                for (int i = 0; i < PairCache.CollisionConstraintTypeCount; ++i)
                {
                    count += pendingConstraintsByType[i].Count;
                }
                return count;
            }

        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        void AddConstraint<TBodyHandles, TDescription, TContactImpulses>(int workerIndex, int manifoldConstraintType, ref CollidablePair pair,
            PairCacheIndex constraintCacheIndex, ref TContactImpulses impulses, TBodyHandles bodyHandles, ref TDescription constraintDescription)
            where TDescription : unmanaged, IConstraintDescription<TDescription>
        {
            overlapWorkers[workerIndex].PendingConstraints.AddConstraint(manifoldConstraintType, ref pair, constraintCacheIndex, bodyHandles, ref constraintDescription, ref impulses);
        }


    }
}
